clc
clear
load matlab.mat;


ss=1;
while( Measurement_Vision(ss,1)<=  num_poses)

    ss = ss+1;
    
end

i=1;

currentpose_id=0;
currentpose_idstr = num2str(currentpose_id);
currentpose_idstrlong = strcat(currentpose_idstr,'.txt');

fileID = fopen(currentpose_idstrlong,'a');
pose = RobotState{1}.pose;
R = pose(1:3,1:3)*expm(skew( randn(3,1)*0.1));
      t = pose(1:3,4)+randn(3,1)*0.3;
      [Alpha,Beta,Gamma] = fn_ABGFromR_Motion( R );
      fprintf(fileID,'%f %f %f %f %f %f\r\n', Alpha,Beta,Gamma, t(1), t(2), t(3) );
      fclose(fileID);   

while( Measurement_Vision(i,1)<=  num_poses)
pose_id = ( Measurement_Vision(i,1) );
pose_index = Measurement_Vision(i,1);

if pose_id > currentpose_id
   currentpose_id = currentpose_id+1;   
   currentpose_idstr = num2str(currentpose_id); 
   currentpose_idstrlong = strcat(currentpose_idstr,'.txt');
   fileID = fopen(currentpose_idstrlong,'w');
      pose = RobotState{pose_index+1}.pose;
      R = pose(1:3,1:3)*expm(skew( randn(3,1)*0.1));
      t = pose(1:3,4)+randn(3,1)*0.3;
      [Alpha,Beta,Gamma] = fn_ABGFromR_Motion( R );
      fprintf(fileID,'%f %f %f %f %f %f\r\n', Alpha,Beta,Gamma, t(1), t(2), t(3) );
      fclose(fileID);   
end

landmark_id =['landmark' num2str( Measurement_Vision(i,2) )];
num_landmark_id = Measurement_Vision(i,2);
num_ob_id = sum(Measurement_Vision(1:ss-1,2)==num_landmark_id);


if num_ob_id>=2
    fileID = fopen(currentpose_idstrlong,'a');
      uv=  (Measurement_Vision(i,3:4))  ;
      landmark_IDnum = Measurement_Vision(i,2);
      landmarks = Landmarks.(landmark_id)+2.3*randn(3,1);
      fprintf(fileID,'%d %f %f 0 0 0\r\n', landmark_IDnum, uv(1), uv(2) ); 
      fclose(fileID);
end


i=i+1;    
end